
#include "Graph.hpp"
#include "Math.hpp"

#include <iostream>
#include <cmath>

using namespace com::toxiclabs::tsoc;
using namespace std;

#define _DEBUG_

Node::Node(vector<Triangle *>* triangle_list,int depth)
{
	
	
	float distx,disty,distz;
	float dist[3];
	
	Vector min;
	Vector max;
	
	float mid;
		
	span_axis = KDTREE_X_SPAN;
	
	
	/* compute bounding box*/
	min=triangle_list->at(0)->vertex[0];
	max=triangle_list->at(0)->vertex[0];
	
	for(int n=0;n<triangle_list->size();n++)
	{
		for(int m=0;m<3;m++)
		{
			if(triangle_list->at(n)->vertex[m].x<min.x)
				min.x=triangle_list->at(n)->vertex[m].x;
			
			if(triangle_list->at(n)->vertex[m].y<min.y)
				min.y=triangle_list->at(n)->vertex[m].y;
			
			if(triangle_list->at(n)->vertex[m].z<min.z)
				min.z=triangle_list->at(n)->vertex[m].z;
			
			if(triangle_list->at(n)->vertex[m].x>max.x)
				max.x=triangle_list->at(n)->vertex[m].x;
			
			if(triangle_list->at(n)->vertex[m].y>max.y)
				max.y=triangle_list->at(n)->vertex[m].y;
			
			if(triangle_list->at(n)->vertex[m].z>max.z)
				max.z=triangle_list->at(n)->vertex[m].z;
			
		}
	}
		
	
	
	/* compute bounding box dimensions */
	
	dist[KDTREE_X_SPAN] = sqrtf( (max.x-min.x)*(max.x-min.x) );
	dist[KDTREE_Y_SPAN] = sqrtf( (max.y-min.y)*(max.y-min.y) );
	dist[KDTREE_Z_SPAN] = sqrtf( (max.z-min.z)*(max.z-min.z) );
	/*
	left[KDTREE_X_SPAN]=min.x;
	left[KDTREE_Y_SPAN]=min.y;
	left[KDTREE_Z_SPAN]=min.z;
	
	right[KDTREE_X_SPAN]=max.x;
	right[KDTREE_Y_SPAN]=max.y;
	right[KDTREE_Z_SPAN]=max.z;
	*/
	/* get best span axis */
	
	if(dist[KDTREE_Y_SPAN]>dist[KDTREE_X_SPAN])
	{
		if(dist[KDTREE_Z_SPAN]>dist[KDTREE_Y_SPAN])
		{
			span_axis=KDTREE_Z_SPAN;
		}
		else
		{
			span_axis=KDTREE_Y_SPAN;
		}
	}
	else
	{
		if(dist[KDTREE_Z_SPAN]>dist[KDTREE_X_SPAN])
		{
			span_axis=KDTREE_Z_SPAN;
		}
	}
	
	span_point=min[span_axis]+(dist[span_axis]/2.0f);
	span_right=max[span_axis];
	span_left=min[span_axis];
#ifdef _DEBUG_
	cout<<"Span axis: "<<span_axis<<endl;
	cout<<"Span point: "<<span_point<<endl;
	cout<<"["<<span_left<<" | "<<span_right<<"]"<<endl;
#endif
	
	Vector diagonal = max - min;
	radius = diagonal.Module()/2.0f;
	
	sphere = (diagonal * 0.5f) + min;
	
	
	cout<<"sphere:"<<sphere.x<<","<<sphere.y<<","<<sphere.z<<","<<sphere.w<<endl;
	
	
	/* this value should be computed , or at least, user defined :s */
	if(depth==7)
	{
#ifdef _DEBUG_
		cout<<"+ node is depth enought"<<endl;
#endif
		for(int n=0;n<triangle_list->size();n++)
		{
			triangles.push_back(triangle_list->at(n));
		}
		
		this->left=NULL;
		this->right=NULL;
		return;
	}
	
		
	vector<Triangle *> triangles_left;
	vector<Triangle *> triangles_right;
	
	for(int n=0;n<triangle_list->size();n++)
	{
		switch(GetTriangleSpanSide(span_axis,span_point,triangle_list->at(n)))
		{
			case KDTREE_SPAN_LEFT:
				triangles_left.push_back(triangle_list->at(n));
			break;
			
			case KDTREE_SPAN_CENTER:
				triangles.push_back(triangle_list->at(n));
			break;
			
			case KDTREE_SPAN_RIGHT:
				triangles_right.push_back(triangle_list->at(n));
			break;
		}
	}
	
#ifdef _DEBUG_
	cout<<"+ node size:"<<triangles.size()<<endl;
	cout<<"+ node right size:"<<triangles_right.size()<<endl;
	cout<<"+ node left size:"<<triangles_left.size()<<endl;
	cout<<"* total: "<<triangle_list->size()<<endl;
	cout<<"* depth: "<<depth<<endl;
#endif
	
	this->left=NULL;
	this->right=NULL;
	
	if(triangles_left.size()>0)
	{
		left = new Node(&triangles_left,depth+1);
	}
	
	if(triangles_right.size()>0)
	{
		right = new Node(&triangles_right,depth+1);
	}
	
	
}

Node::~Node()
{
}

int Node::GetTriangleSpanSide(int axis,float pos,Triangle * triangle)
{
	float a,b,c; 
	int left=0;
	int right=0;
	int coplanar=0;
	
	a=triangle->vertex[0][axis]-pos;
	b=triangle->vertex[1][axis]-pos;
	c=triangle->vertex[2][axis]-pos;
	
	if(AproxToZero(a))
	{
		coplanar++;
	}
	else
	{
		if(a>0.0f)
			right++;
		else
			left++;
	}
	
	if(AproxToZero(b))
	{
		coplanar++;
	}
	else
	{
		if(b>0.0f)
			right++;
		else
			left++;
	}
	
	if(AproxToZero(c))
	{
		coplanar++;
	}
	else
	{
		if(c>0.0f)
			right++;
		else
			left++;
	}
	

	if(left>0 && right>0)
	{
		return KDTREE_SPAN_CENTER;
	}
	
	if(left>0)
	{
		return KDTREE_SPAN_LEFT;
	}
	
	if(right>0)
	{
		return KDTREE_SPAN_RIGHT;
	}
	
	if(coplanar==3)
	{
		return KDTREE_SPAN_CENTER;
	}

	return KDTREE_SPAN_CENTER;
}


float Node::GetTriangleCenter(int axis,Triangle * triangle)
{
	float mid;
	
	mid = triangle->vertex[0][axis];
	mid+=triangle->vertex[1][axis];
	mid+=triangle->vertex[2][axis];
	
	return mid/3.0f;
}




int Node::Intersect(Vector &origin,Vector &direction)
{
	int ret = 0;
	
	Vector pnormal[3];
	
	pnormal[0].Set(1.0f,0.0f,0.0f,0.0f);
	pnormal[1].Set(0.0f,1.0f,0.0f,0.0f);
	pnormal[2].Set(0.0f,0.0f,1.0f,0.0f);
	
	
						
	float v = pnormal[span_axis] * direction;
	
	
	/* ray is not parallel */
	if(!AproxToZero(v))
	{
		/* right side */ 
		if(origin[span_axis]>span_point)
		{			
			if(origin[span_axis]<span_right)
			{
				ret = KDTREE_INTERSECT_RIGHT;
			}
			if(v<0.0f)
			{
				ret = KDTREE_INTERSECT_RIGHT | KDTREE_INTERSECT_LEFT;
			}
		}
		else /* left side */
		{
			if(origin[span_axis]>span_left)
			{
				ret = KDTREE_INTERSECT_LEFT;
			}
			if(v>0.0f)
			{
				ret = KDTREE_INTERSECT_RIGHT | KDTREE_INTERSECT_LEFT;
			}
		}
		
		
	}
	
	
	
	
	return ret;
	
}

bool Node::SphereTest(Vector &origin,Vector &direction)
{
	Vector C;
		
	C=origin-sphere;
	float a = direction * direction;
	float b = 2.0f * (C * direction);
	float c = (C * C) - (radius * radius);
	
	float root = (b*b) - (4.0f*a*c);
	if(root<0.0f)return false;
	
	root=sqrt(root);
	float t1=(-b+root)/(2.0f*a);
	float t2=(-b-root)/(2.0f*a);
	
	if(t1<0.0f && t2<0.0f)return false;
	
	return true;
	
}

Graph::Graph(std::vector<Triangle *>* triangle_list)
{
		cout<<"* Building kd-tree..."<<endl;
		root = new Node(triangle_list,0);

#ifdef _DEBUG_
		cout<<"* Analyzing kd-tree..."<<endl;	
		AnalyzeGraph();
#endif
		
}

void r_Analyze(Graph * graph,Node * node)
{
	if(node==NULL)return;
	
	if(node==graph->root)
	{
		cout<<"Root: "<<node->triangles.size()<<endl;
	}
	else
	{
		if(node->left==NULL && node->right==NULL)
		{
			cout<<"Leaf: "<<node->triangles.size()<<endl;
		}
		else
		{
			cout<<"Node: "<<node->triangles.size()<<endl;
		}
	}
	
	
	r_Analyze(graph,node->left);
	r_Analyze(graph,node->right);
}

void Graph::AnalyzeGraph()
{
	r_Analyze(this,root);
}

void r_Traverse(Vector &origin,Vector &direction,Node * q,vector<Triangle *> * triangles)
{
	if(q==NULL)return;
	
	int intersection = q->Intersect(origin,direction);
	
	if(intersection & KDTREE_INTERSECT_LEFT)
		r_Traverse(origin,direction,q->left,triangles);
	
	if(intersection & KDTREE_INTERSECT_RIGHT)
		r_Traverse(origin,direction,q->right,triangles);
	
	if(intersection!=0)
	{
		if(q->SphereTest(origin,direction))
		{
			vector<Triangle *>::iterator it;
			for(it=q->triangles.begin();it!=q->triangles.end();it++)
			{
				triangles->push_back(*it);
			}
		}
	}
}

void Graph::Traverse(Vector &origin,Vector &direction,vector<Triangle *> * triangles)
{
	r_Traverse(origin,direction,root,triangles);
}

/*!
 * Thomas and Ben Intersection
 */ 
bool Graph::RayTriangle(Triangle * tri,Vector &origin,Vector &direction,Vector * collision)
{
	
	Vector vab;
	Vector vac;
	
	float cosAlpha;
	
	float D;
	float deltaD;
	float t;
	
	Vector col;
	Vector Nv;
	
	int i0,i1,i2;
	float u0,u1,u2,v0,v1,v2;

	float alpha,beta;
	
	Vector & trNormal = tri->pnormal;
	
	bool ret=false;
	 
	
	//first check
	
	
	cosAlpha = direction * trNormal;
	
	/*
	 * backface culling is sweety, but it won't have any sense when transparency arrives 
	if(cosAlpha>=0.0f)
		return false;
	*/
	
	//D=A*normal
	D = tri->vertex[0] * trNormal;
	
	
	// deltaD = D - origin*normal;
	deltaD = D - (origin * trNormal);
	
	//t=(deltaD/cosAlpha);
	t=deltaD/cosAlpha;
	
	//plane collision test
	if(t>=0.0f)
	{
		//col = origin + direction*t;
		col = (direction * t) + origin;
		
			
		/*
		Nv[0]=fabs(trNormal[0]);
		Nv[1]=fabs(trNormal[1]);
		Nv[2]=fabs(trNormal[2]);
		*/
		Nv=trNormal;
		Nv.Abs();
		
		/*
		if(Nv.v[0]>max(Nv.v[1],Nv.v[2])){i0=0;i1=1;i2=2;}
		if(Nv.v[1]>max(Nv.v[0],Nv.v[2])){i0=1;i1=0;i2=2;}
		if(Nv.v[2]>max(Nv.v[0],Nv.v[1])){i0=2;i1=0;i2=1;}
		*/
	
		/*
		if(Nv[0]>Maxf(Nv[1],Nv[2]))
		{
			i0=0;
			i1=1;
			i2=2;
		}
		else
		{
		
			if(Nv[1]>Maxf(Nv[0],Nv[2]))
			{
				i0=1;
				i1=0;
				i2=2;
			}
			else
			{
				i0=2;
				i1=0;
				i2=1;
			}
		}
		*/
		
		int c1 = Nv[0]>Maxf(Nv[1],Nv[2]);
		int c2 = Nv[1]>Maxf(Nv[0],Nv[2]);
		
		i0 = 2 - (2*c1) - c2;
		i1 = c1;
		i2 = 3 - i0 - i1;
		
		u0=col[i1] - tri->vertex[0][i1];
		v0=col[i2] - tri->vertex[0][i2];
		
		u1=tri->vertex[1][i1] - tri->vertex[0][i1];
		u2=tri->vertex[2][i1] - tri->vertex[0][i1];
		
		v1=tri->vertex[1][i2] - tri->vertex[0][i2];
		v2=tri->vertex[2][i2] - tri->vertex[0][i2];
		
		if(AproxToZero(u1))
		{
			beta = u0/u2;
			alpha = (v0-beta*v2)/v1;
			
			
		}
		else
		{
			beta = (v0*u1 - u0*v1)/(v2*u1 - u2*v1);
			alpha = (u0 - beta*u2)/u1;
			
			
		}
		
		ret = (beta>=0.0f && beta<=1.0f) && (alpha>=0.0f && (alpha+beta)<=1.0f);
		*collision=col;		
	}
	
	return ret;
}
